package com.gitee.wsl.unit.angle.ext

import com.gitee.wsl.math.Math
import com.gitee.wsl.unit.angle.AngleUnit
import com.gitee.wsl.unit.angle.toRadians
import kotlin.math.PI


/** Linearly interpolates between two angles in radians. Takes into account that angles wrap at two pi and always takes the
 * direction with the smallest delta angle.
 *
 * @param fromRadians start angle in radians
 * @param toRadians target angle in radians
 * @param progress interpolation value in the range `[0, 1]`
 * @return the interpolated angle in the range `[0, PI2]`
 */
fun lerpAngle(fromRadians: Double, toRadians: Double, progress: Double): Double {
    val delta = (toRadians - fromRadians + Math.TWO_PI + PI) % Math.TWO_PI - PI
    return (fromRadians + delta * progress + Math.TWO_PI) % Math.TWO_PI
}

fun lerpAngle(fromRadians: AngleUnit, toRadians: AngleUnit, progress: Double)=
    lerpAngle(fromRadians.radian,toRadians.radian,progress).toRadians()